#ifndef TOARDUINOA_HPP
#define TOARDUINOA_HPP

#include <stdio.h>
#include <unistd.h>			//Used for UART
#include <fcntl.h>			//Used for UART
#include <termios.h>		//Used for UART
#include <string.h>
#include <iostream>

#include <thread>
#include <chrono>
#include <mutex>
#include <atomic>

class ToArduino
{
public:
    int uart0_filestream;
public:
    ToArduino()
        : uart0_filestream(-1)
    {

    }
};

class ToArduinoA : public ToArduino
{
public:
    ToArduinoA()
        : ToArduino()
    {

    }

    bool Initial(const char *file="/dev/ttyAMA0", tcflag_t baudrate=B115200)
    {
        uart0_filestream = open(file, O_RDWR | O_NOCTTY | O_NDELAY);		//Open in non blocking read/write mode

        if (uart0_filestream == -1)
        {
            //ERROR - CAN'T OPEN SERIAL PORT
            printf("Error - Unable to open UART.  Ensure it is not in use by another application\n");
            return false;
        }

        struct termios options;
        tcgetattr(uart0_filestream, &options);
        options.c_cflag = CS8 |
                baudrate | //<Set baud rate
                CLOCAL |
                CREAD;
        options.c_iflag = IGNPAR;
        options.c_oflag = 0;
        options.c_lflag = 0;
        tcflush(uart0_filestream, TCIFLUSH);
        tcsetattr(uart0_filestream, TCSANOW, &options);
        return true;
    }

    int SendMsgUChar(const unsigned char *sendstr, size_t n)
    {
        int count=write(uart0_filestream, sendstr, n); //Filestream, bytes to write, number of bytes to write
        if (count < n)
        {
            printf("UART TX error\n");
        }
        return count;
    }


    int GetMsgUChar(unsigned char *getstr, size_t n)
    {
        int rx_length = read(uart0_filestream, (void*)getstr, n);		//Filestream, buffer to store in, number of bytes to read (max)
        if (rx_length < 0)
        {
            //An error occured (will occur if there are no bytes)
            //            printf("UART RX error\n");
        }
        return rx_length;
    }
#if 1
    void test()
    {
        if(!Initial("/dev/ttyUSB0", B9600))
        {
            //ERROR - CAN'T OPEN SERIAL PORT
            printf("Error - Unable to open UART.  Ensure it is not in use by another application\n");
            return;
        }

        unsigned char buf[32];

        for(int i=0;i<10000;i++)
        {
            bzero(buf,32);
            //            std::cout<<"try "<<i<<"\n"<<std::flush;
            usleep(5e3);
            if(GetMsgUChar(buf, 32)>0)
                std::cout<<buf<<std::flush;

        }
    }
#endif

    static void PrintUchar(const unsigned char* msg, unsigned int length)
    {
        for(unsigned int i=0;i<length;i++)
            std::cout<<((int)msg[i])<<", ";
        std::cout<<"\n"<<std::flush;
    }


};


class ToST : public ToArduinoA
{
public:
    std::string state;
    std::mutex stateLock;
    std::thread th;
    std::atomic<bool> flag;

public:
    ToST()
        : ToArduinoA()
        , state("RESET")
    {

    }



    void UpdateState(const char *file="/dev/ttyACM0", tcflag_t baudrate=B115200, unsigned buflen=32)
    {
        if(!Initial(file, baudrate))
        {
            //ERROR - CAN'T OPEN SERIAL PORT
            printf("Error - Unable to open UART.  Ensure it is not in use by another application\n");
            return;
        }

        unsigned char *buf=new unsigned char[buflen];

        std::string lastbuf;

        while(flag)
        {
            bzero(buf,buflen);
            int n=GetMsgUChar(buf, buflen);
            if(n>0)
            {
//                std::cout<<buf<<std::flush;
                lastbuf+=std::string(buf, buf+n);
                std::size_t it = lastbuf.find('\n');
                while (it != std::string::npos)
                {
                    stateLock.lock();
                    state=lastbuf.substr(0,it-1);
                                        std::cout<<"state=["<<state<<"]\n"<<std::flush;
                    stateLock.unlock();
                    lastbuf.erase(0,it+1);
                    it = lastbuf.find('\n');
                }
                                std::cout<<"state end=["<<state<<"]\n"<<std::flush;
            }
            std::this_thread::yield();

        }
        delete []buf;
    }

    static void UpdateStateThread(ToST *pdata)
    {
        pdata->UpdateState("/dev/ttyACM0", B9600);
    }

    void StartUpdate()
    {
        state="b";
        flag=true;
        th=std::thread(UpdateStateThread, this);
    }

    void EndUpdate()
    {
        flag=false;
        if(th.joinable()) th.join();
    }



    bool IsStop()
    {
        stateLock.lock();
        bool res=(state.compare("b")!=0);
        stateLock.unlock();
        return res;
    }

    bool IsAGVStop()
    {
        stateLock.lock();
        bool res=(state.compare("6")==0);
        stateLock.unlock();
        return res;
    }


    bool IsAGVStop2()
    {
        stateLock.lock();
        bool res=(state.compare("9")==0);
        stateLock.unlock();
        return res;
    }

    bool IsAGVStop3()
    {
        stateLock.lock();
        bool res=(state.compare("3")==0);
        stateLock.unlock();
        return res;
    }



    void test()
    {
        StartUpdate();
#if 0
        for(int i=0;i<50;i++)
        {
            usleep(1e6);
//            std::cout<<(IsStop()?"stop\n":"run\n")<<std::flush;
//            char msg[4]="p\n";
//            SendMsgUChar((unsigned char*)msg,1);

            unsigned char num=i;
            SendMsgUChar(&num,1);

            usleep(1e6);
        }
#endif

        usleep(1e7);
        std::cout<<"send\n"<<std::flush;
        unsigned char num='8';
        SendMsgUChar(&num,1);

        usleep(1e7);

//        while(!IsAGVStop()){ std::this_thread::yield(); }



        EndUpdate();
    }

};



#endif // TOARDUINOA_HPP
